LOAN  DOCUMENT 


19990519  131 

DATE  RECEIVED  IN  DTIC  REGISTERED  OB  CERTIFIED  NUMBER 


D'ncJS^70A 


PHOTOGRAPH  THIS  SHEET  AND  RETURN  TO  DTIC-FDAC 


DOCUMENT  PROCESSING  SHEET 

LOAN  DOCUMENT 


mm  UaV  mm  * 

STOQC  IS  EXHAUSTED. 


a  <  g  p  -a  m  g  >1  h  a 


RIA-84-U322 


- 1  euimiual 

Paper 

2341 

July  1984 


L.  Keith  Barker 
and  Mary  C.  Moore 


NASA 

Technical 

Paper 

2341 

1984 


Kinematic  Control 
of  Robot  With 
Degenerate  Wrist 


L.  Keith  Barker 
and  Mary  C.  Moore 

Langley  Research  Center 
Hampton,  Virginia 


NASA 

National  Aeronautics 
and  Space  Administration 


Scientific  and  Technical 
Information  Branch 


SUMMARY 


A  popular  method  of  controlling  a  robot  arm  is  to  use  kinematic  resolved-rate 
equations,  which  allow  an  operator  to  dynamically  control  the  arm  by  using  visual 
feedback.  In  using  resolved-rate  equations,  an  operator  commands  robot  hand  veloc¬ 
ity,  which  is  resolved  into  joint  angle  rates  to  cause  the  commanded  motions.  By 
visual  feedback,  the  operator  adjusts  these  commands. 

The  robot  wrist  consists  of  three  rotational  joints  in  the  robot  arm.  In  this 
paper,  the  wrist  loses  a  degree  of  freedom  (degenerate  condition)  when  the  second 
wrist  joint  is  at  0°  so  that  the  first  and  third  wrist  joints  have  colinear  rota¬ 
tional  axes.  Methods  are  analyzed  for  controlling  the  robot  hand  at  or  near  this 
degenerate  wrist  condition. 

The  resolved-rate  equations  become  indeterminant  when  the  robot  wrist  is  degen¬ 
erate,  and  slightly  away  from  this  condition,  very  large  joint  angle  rates  are  calcu¬ 
lated.  From  a  practical  viewpoint,  the  joints  cannot  rotate  faster  than  some  maximum 
speed.  Therefore,  resolved-rate  control  with  rate  limiting  is  examined.  Whenever  a 
computed  joint  angle  rate  exceeds  a  specified  maximum  value,  the  maximum  value  is 
used.  It  is  shown  that  resolved-rate  control  with  rate  limiting  produces  numerous 
unexpected  responses  in  the  degenerate  wrist  region.  The  generalized  matrix  inverse 
solution  to  the  resolved-rate  equations  can  also  produce  unwanted  responses  to  com¬ 
manded  inputs. 

A  new  method  is  introduced  to  control  the  robot  hand  in  the  region  of  the  degen¬ 
erate  wrist.  The  method  uses  a  coordinated  movement  of  the  first  and  third  joints  of 
the  robot  wrist  to  locate  the  second  wrist  joint  axis  for  movement  of  the  robot  hand 
in  the  commanded  direction.  This  method  does  not  entail  infinite  joint  angle  rates 
or  cause  extraneous  hand  movements;  however,  there  is  a  brief  delay  for  the  coordi¬ 
nated  movement  (which  is  accomplished  in  an  optimal  manner). 


INTRODUCTION 

Controlling  individual  joints  in  a  robot  arm  to  accomplish  a  complex  task  is 
difficult,  especially  if  time  to  complete  the  task  is  critical  or  if  part  of  the 
operator's  attention  is  needed  elsewhere.  A  more  natural  approach  is  for  an  operator 
to  command  the  motion  of  the  robot  hand  and  then  automate  the  requisite  coordination 
of  the  individual  joints  in  the  arm.  An  operator  can  use  kinematic,  resolved-rate 
equations  (ref.  1)  to  dynamically  control  a  robot  arm  by  watching  its  response  to 
commanded  inputs.  The  operator  views  the  robot  hand,  decides  that  he  wants  it  to 
move  in  a  certain  direction,  and  deflects  a  controller.  The  robot  hand  then  moves 
accordingly  with  a  velocity  proportional  to  the  amount  of  deflection  of  the  control¬ 
ler.  Commanded  hand  velocities  are  transformed  (resolved)  into  requisite  movements 
(velocities)  of  the  individual  joints  in  the  robot  arm  to  effect  the  commanded  hand 
motion. 

Three  rotational  joints  in  the  robot  arm  constitute  the  robot  wrist.  Whenever 
any  two  of  these  joints  have  their  rotational  axes  colinear,  the  wrist  loses  a  degree 
of  freedom  and  is  said  to  be  degenerate.  In  this  condition,  the  resolved-rate  equa¬ 
tions  are  indeterminant.  In  close  proximity  to  the  degenerate  condition,  calculated 


joint  angle  rates  can  exceed  maximum  operational  limits.  The  emphasis  in  reference  2 
is  on  predicting  and  avoiding  such  degenerate  regions;  however,  this  reduces  the 
working  regions  of  the  robot  wrist  and  requires  the  arrangement  of  tasks  to  keep  the 
wrist  out  of  the  degenerate  orientation.  The  intent  in  this  paper  is  to  continue 
controlling  the  robot  hand  at  (or  near)  the  degenerate  wrist  condition  to  allow  more 
flexible  movement  geometry. 
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homogeneous  transformation  matrix  from  coordinate  system  i  to  i-1 
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• 

L  times  sign  of  Q.  in  equation  (16) 

o 

integer  to  indicate  different  axis  systems  and  associated  parameters 
Jacobian  matrix 

integer  corresponding  to  value  used  in  optimal  coordinated  movement 
maximum  joint  angle  rate  limit 
L  times  sign  of  in  equation  (15) 

origin  of  joint  i  axis  system 
vector  to  point  Q 

rotational  transformation  matrix  from  coordinate  system  i  to  i-1; 

3x3  matrix  in  upper  left  corner  of 

inverse  (transpose)  of 

rotational  transformation  matrix  from  coordinate  system  i  to  hand  coordi¬ 
nate  system  (see  eq.  (11)) 

distance  between  coordinate  systems  i  -  1  and  i  along  Z^  1 
time 

linear  velocity  of  robot  hand 

* 

commanded  linear  velocity  of  robot  hand 
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magnitude  of  V  ,  V  ,  and  V  ,  respectively 
X  Y  Z 

linear  velocity  components  of  robot  hand  (in  hand  axis  system) 
forearm  axis  parallel  to  X3 ,  Y3,  and  Z3,  respectively 

axis  directed  along  common  normal  between  Z^_^  and  Z^ 
axis  directed  to  complete  right-hand  axis  system  with  X^  and  Z^ 
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axis  of  rotation  of  joint  i  +  1 
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■> 

angle  between  and 

candidate  variation  in  ©4  for  coordinated  movement 

minimal  variation  chosen  for  04  in  coordinated  movement 

joint  angle  with  initial  value  corresponding  to  position  of  robot  arm  in 
figure  2 

joint  angle  between  Xi_1  and  Xi?  measured  positively  about  positive 
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value  for  9^  corresponding  to  value  (|>^  for  04  in  coordinated  movement 
optimal  \  ^  used  in  coordinated  movement 
candidate  value  for  04  in  coordinated  movement 
optimal  used  in  coordinated  movement 

commanded  rotational  velocity  of  robot  hand  in  plane  of  hand  axes 

X6  and  Y6 

resultant  rotational  velocity  of  robot  hand  (in  hand  axis  system) 
commanded  rotational  velocity  of  robot  hand  (in  hand  axis  system) 
magnitude  of  u>x,  wy,  and  u>z,  respectively 

rotational  velocity  components  of  robot  hand  (in  hand  axis  system) 

rotational  velocity  of  robot  hand  caused  by  rotation  of  joints  1,  2,  and  3 
(in  hand  axis  system) 

rotational  velocity  of  robot  hand  caused  by  rotation  of  three  wrist 
joints  4,  5,  and  6  (in  hand  axis  system) 


Subscript: 

c  value  commanded  by  operator 

* 

Abbreviations: 

E  elbow  of  robot  arm 

ES  elbow-to-shoulder  length 

H  hand  of  robot  arm 

HW  hand-to-wrist  length 
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N  neck  of  robot  arm 

NO  neck-to-base  length 

0  base  of  robot  arm 

S  shoulder  of  robot  arm 

SN  shoulder-to-neck  length 

W  wrist  of  robot  arm 

WE  wrist-to-elbow  length 

Use  of  a  dot  over  a  symbol  indicates  the  first  derivative  with  respect  to  time. 

ANALYSIS 

Resolved— rate  control  enables  an  operator  to  effectively  issue  commands  (such  as 
"go  forward"  or  "pitch  up")  to  the  robot  hand  by  commanding  translational  and  rota¬ 
tional  velocities  in  the  hand's  own  axis  system.  These  commanded  velocities  are 
resolved  by  transformation  matrices  into  joint  angle  rates  along  the  robot  arm. 


Joint  Axis  Systems  and  Transformation  Matrices 

Consecutive  joint  axis  systems  in  robotic  manipulators  can  be  related  by  the 
Denavit-Hartenberg  parameters  (ref.  3).  For  rotational  joints  (fig.  1),  these 
parameters  consist  of  three  constant  parameters  a^,  r.  ,  and  a.  and  a  variable 
joint  angle  0j.  By  definition,  joints  always  rotate  about  their  Z-axis.  The 
Yi_1 -axis  and  Y^-axis  (not  shown)  complete  right-handed  coordinate  systems. 

The  parameter  r^  plays  the  role  of  s^  in  reference  4  and  d^  in  reference  3. 

The  homogeneous  transformation  matrix  (based  on  fig.  1)  from  coordinate  system 
i  to  coordinate  system  i  -  1  is  (ref.  4  or  5,  for  example) 
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Let  the  location  of  a  point  Q  with  respect  to  the  coordinate  system  (X^Y^Z^)  be 
described  by  the  vector  Then,  the  location  of  Q  from  coordinate  system 

(xi-1  ,Yi-i  )  is  the  vector  ,  where 
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in  which  A*  accounts  for  both  rotation  and  displacement  of  (X^,Y^,Z^)  with 
respect  to  »Yi_-|»zi-1  )•  However,  if  one  is  only  interested  in  the  components 

of  q-  in  directions  parallel  to  (X^_i  ,Y£_-|  , Z^_-|  ),  such  as  velocity,  or  if  adjacent 
axis  systems  have  coincident  origins,  then  it  is  sufficient  to  compute 


4-i  =  Ri-i4 
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Figure  2  is  a  modification  of  a  figure  in  reference  4  and  illustrates  a  robot 
arm  and  joint  axis  systems.  The  Denavit-Hartenberg  parameters  and  joint  angle  limits 
assumed  in  reference  4  (and  here)  are  shown  in  table  I.  Notice  that  0^  is  related 
to  another  joint  angle  0-j.  (unprimed).  The  joint  angles  Q±  (i  =  1,  2,  ...,  6)  are 
referenced  to  the  initial  position  of  the  robot  arm  in  figure  2. 

The  transformation  matrix  (in  terms  of  9^,  i  =  1,  2,  •»«,  6)  is  given 

in  reference  4  and  repeated  in  appendix  A.  The  rotational  matrix  is  simply 

the  3x3  submatrix  in  the  upper  left-hand  corner  of  A^. 

The  axis  for  the  robot  hand  may  be  located  wherever  desired  for  convenience,  for 
example,  near  the  tip  of  the  robot  hand,  at  the  robot  hand  mounting  or,  as  in  refer¬ 
ence  4  and  here,  at  the  origin  of  the  wrist  axis  system  (HW  =0).  In  the  sequel, 
commands  of  rotational  and  translational  velocity  to  the  robot  hand  are  expressed  in 
terms  of  joint  angle  rates. 

Equations  for  Resolved-Rate  Control 

As  indicated  in  figure  3,  an  operator  commands  the  translational  velocity 


and  the  rotational  velocity 


(6) 
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of  the  robot  hand  (in  the  hand  axis  system).  On  the  basis  of  visual  and  probably 
force  feedback,  the  operator  varies  these  inputs.  Equations  which  relate  the  opera¬ 
tor's  inputs  for  the  hand  motion  (velocity)  to  joint  angle  rates  in  the  robot  arm  are 
based  on  the  transformation  matrices  between  joint  axis  systems. 


Translational  Velocity  of  Robot  Hand 

As  in  reference  4,  it  is  assumed  that  HW  =  0  and  that  translational  velocity 
of  the  robot  hand  is  attributed  entirely  to  the  first  three  joint  angle  rates  9  , 

02,  and  Gy  Thus,  1 

r. 

r 

*  -  J\52  /  (7) 


where  J  is  an  appropriate  Jacobian  matrix  (ref.  4).  Equation  (7)  is  solved  for  the 
joint  angle  rates  0^  0  and  0^  There  are  singularities  (refs.  4  and  6)  associ¬ 
ated  with  the  matrix  J,  But  these  are  not  considered  here. 


Rotational  Velocity  of  Robot  Hand  (In  Hand  Axis  System) 
The  rotational  velocity  of  the  robot  hand  is 


-> 

0) 
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-* 
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where 


(8) 


(9) 


is  the  rotational  velocity  caused  by  the 


first  three  joints  in  the  robot  arm,  and 


(10) 
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is  the  rotational  velocity  caused  by  the  wrist  joints.  Note  that  the  transpose 
(inverse)  of  equation  (4)  is  used  in  equations  (9)  and  (10).  In  equations  (9) 
and  (10), 


i  5  4  i 

R  —  R  R  * .  •  R .  . 

6  6  5  1+1 


(11  ) 


is  a  product  of  rotational  transformation  matrices,  which  are  obtained  from  the 
transpose  of  equation  ( 4 ) • 


Rotational  Speeds  of  Robot  Wrist  and  Hand 
Express  equation  (8)  as 


->  ->  + 

W.  c  tz  =  <*>  -  U)  0  ~ 

4,5,6  C  1  r  2, 3 


(12) 


•  • 

where  c5c  denotes  the  commanded  value  for  The  joint  angle  rates  0^ ,  Q^t  and 

0^  needed  to  compute  &1,2, 3  are  obtained  from  equation  (7).  Hence,  equation  (12) 

gives  the  required  ^4,5,6  needed  to  generate  (j5c»  For  simplicity  in  this  analysis, 

qi=02  =  93  =  O  so  that  <&i,2,3  =  an<a  equation  (12)  becomes 


->• 

to 


4,5,6 


-> 


(13) 


Wrist  Speed  From  Commanded  Hand  Rotation 
Equation  (13)  is  equivalent  to  the  three  scalar  equations  (see  appendix  B) 


9 5  =  (Vc  Sin  9 6  +  (<l)Y ’c  COS  9 6 


(14) 


0.  sin  0C  =  —  (to  )  cos  0C  +  (to  )  sin  0 

4  5  Xc  o  xc  d 


(15) 


9 6  =  (Vc  -  94  C°S  9 5  (16 

Equation  (14)  clearly  shows  that  there  is  no  dif f iculty ^in  computing  9^,  and  equa¬ 
tions  (15)  and  (16)  show  that^dif f iculty  in  computing  9^  and  9^  occurs  only  when 
sin  95  =  0.  Computation  of  9^  and  9^  when  sin  9^  =  0  is  the  basic  problem 
addressed  in  this  paper. 
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Computed  Hand  Rotational  Rates 


With  <*>1,2,3  =  0'  equation  (12)  relates  hand  rotational  rates  to  joint  angle 
rates  (see  appendix  B)  as 


\  m  -®4  Si"  %  COS  06  +  Sln  S6 
=  04  sin  05  sin  0g  +  cos  0g 

%  =  ®4  COS  e5  +  K 


(17) 

(18) 

(19) 


Of  course,  identities  result  when  equations  (14)  to  (16)  are  substituted  into  equa¬ 
tions  (17)  to  (19)  when  the  commanded  and  actual  hand  rates  are  equal.  However, 
equation  (15)  is  useless  in  computing  0  when  the  wrist  is  degenerate  (sin  05  =  0), 
and  another  means  of  computing  0^  is  needed.  Whatever  values  of  8  ,  8  ,  and  8 
are  used  in  the  degenerate  region,  equations  (17)  to  (19)  give  the  hand  rotational  6 
rates  that  result  from  these  wrist  joint  angle  rates. 


Methods  for  Controlling  Robot  Hand  in  Degenerate  Wrist  Region 

Joint  variables  can  be  restricted  to  keep  the  robot  wrist  away  from  its  degener¬ 
ate  position  (sin  @5=0).  But,  the  approach  in  this  paper  is  to  try  to  continue 
controlling  the  robot  hand  even  though  the  robot  wrist  approaches  or  reaches  its 
degenerate  state. 


Resolved-Rate  Control  With  Maximum  Rate  Limit 


Practically,  a  robot  arm  is  limited  in  the  speed  with  which ^its  joints  can  move. 
To  represent  this  mathematically,  anytime  the  joint  angle  rate  0  in  equation  (15) 
has  a  magnitude  greater  than  a  specified  operational  limit  L,  replace  0.  with  M, 
which  is  L  times  the  sign  of  @4  in  equation  (15).  Likewise,  when  8  4  in  equa¬ 
tion  (16)  has  a  magnitude  greater  than  L,  replace  it  with  G,  which  is  6L  times  the 
sign  of  0  in  equation  (16).  Thus,  as  sin  05  +  0,  equations  (15)  and  (16)  are 
replaced  by 


If  |04|  >  L 


If 

If 


(20) 


(21) 


Substituting  equations  (14),  (20),  and  (21)  into  equations  (17)  to  (19)  gives  the 
resulting  hand  rates 
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Y  c 


sin  0 
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(0  =  -M  sin  0  cos 
X  5 


(22) 


(23) 


U)  =  M  sin  0_  sin  0.  +  [(u)  )  sin  0  +  (u)  )  cos  0  ]  cos  0 

Y  5  6  L  X  c  6  Y  c  6  i 


0)  =  I 

z  ( 


<v. 


0C  +  G 

D 

If 

l5J 

>  L 

and 

l*«l 

> 

1  ^  t 

1  i  1 

If 

l04l 

>  L, 

but 

< 

(24) 


In  equation  (24),  the  computed  speed  is  equal  to  the  commanded  speed  (wz)c 

if  0.  reaches  its  limit  but  9-  does  not.  If  0.  and  both  reach  the  limit, 

4  .  .  ,  r\  O  4  b 

only  varies  with  v  . 


Equations  (22)  and  (23)  in  nondimensional  form  are 


M 


06  + 


M 


COS 


where 


(25) 


(26) 


E  =  -sin  0_  cos  0^ 
X  5  6 


(27) 


E  =  sin  0_  sin  0^ 
Y  5  6 


(28) 


Equations  (25)  and  (26)  express  the  nondimensional  rotational  speeds  o^/M  an^ 

Uty/M  as  functions  of  the  commanded  nondimensional  rotational  speeds  (w^/M)c  and 
(wy/M)c  for  wrist  joint  angles  and  06.  These  equations  are  plotted  in  fig¬ 
ure  4  for  65  =  0°  (degenerate  condition)  and  selected  values  of  Qg.  For  other 

values  of  0g,  simply  add  the  results  of  equations  (27)  and  (28).  Suppose 

( 00^/ M)c  =  0.  Then  in  figure  4(a),  if  0g  =  ±90°  or  ±270° ,  the  computed  value  a^/M 
always  equals  the  commanded  value  (u^/M)c.  Whereas,  if  0g  =  0°  or  +180°,  o^/M 

always  equals  0  regardless  of  the  commanded  speeds,  and  the  robot  hand  will  not 

respond  at  all  to  commands. 

Suppose  ( u^/M)  c  =  0  and  (u^/M)c  =  1.  Then,  for  06  =  45°  or  -135°  in  fig¬ 
ure  4(b),  =  0.5  and  Gty/M  =  0.5.  In  other  words,  an  operator  commands  a  sole 

rotational  speed  about  the  Y-axis  of  the  robot  hand  and,  in  return,  gets  rotational 
speeds  about  both  the  X-axis  and  Y-axis  of  the  robot  hand.  As  seen  by  a  stationary 
observer,  this  coupling  would  cause  a  conical  motion  of  the  robot  hand  (Zg  in 
fig.  2).  Such  motions  have  been  observed  in  trial  laboratory  tests  and  were  thought 
to  result  entirely  from  inexact  Denavit-Hartenberg  parameters?  however,  as  shown 
here,  this  type  of  motion  can  result  from  using  resolved-rate  control  with  rate 
limiting.  Obviously,  other  methods  must  be  considered  for  controlling  a  robot  hand 
with  a  degenerate  wrist  to  avoid  erratic  behavior. 
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Generalized  Matrix  Inverse 


There  is  no  problem  in  calculating 
and  (16)  may  be  solved  as 


6  from  equation  (14),  and  equations  (15) 
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r  1 

f4V 

sin  9,. 

0 
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-( Vc  COS  e6  +  (Vc  Sin  G6  1 

\y 

cos  9_ 
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K) 

Z  c 

v  J 

(29) 


where  the  asterisk  (*)  denotes  the  generalized  matrix  inverse. 

There  is  a  tendency  to  think  that  the  generalized  inverse  of  a  matrix  is  the 
cure  for  all  ills  caused  by  singularities  because  the  inverse  always  exists  and  is 
unique?  however,  the  solution  formulated  still  may  not  be  acceptable.  In  general, 
several  factors  should  be  kept  in  mind  when  attempting  to  apply  the  generalized 
matrix  inverse:  (1)  real-time  application  may  be  hindered  because  it  takes  too  long 
to  compute  the  inverse;  (2)  the  elements  of  the  inverse  are  not  continuous  functions 
of  the  elements  of  the  matrix  which  is  inverted,  and  there  are  jumps  when  the  matrix 
changes  rank  (ref.  7);  and  (3)  for  some  kinematically  redundant  manipulators,  control 
based  on  generalized  matrix  inverses  may  lead  to  undesirable  arm  configurations 
(ref.  8).  Later,  an  example  is  given  which  shews  that  unwanted  responses  can  be  gen¬ 
erated  by  equation  (29). 


Coordinated  Movement  of  Robot  Wrist 

In  figure  2,  the  robot  wrist  is  degenerate  because  Z3  and  Z5  are  parallel, 
and  there  is  a  problem  with  the  resolved-rate  equations  because  65  =  0°  in  equa¬ 
tion  (15).  Forget  about  the  resolved-rate  equations  for  a  moment  and  suppose  an 
operator  simply  wants  to  rotate  the  robot  hand  about  X6  in  figure  3.  How  can  this 
be  done  without  introducing  extraneous  motions  of  the  robot  hand?  One  way,  for  exam¬ 
ple,  is  to  simultaneously  make  64  =  -90°  and  0g  =  90°  by  varying  joints  4  and  6 
at  the  same  rates  so  that  the  robot  hand  does  not  rotate.  Then,  vary  65  with 
joint  5  at  the  desired  hand  rate.  This  is  the  basic  idea  of  the  coordinated  movement 
discussed  in  this  section.  As  the  wrist  moves  away  from  the  degenerate  condition, 
the  resolved-rate  equations  are  again  used. 

Forearm  axis  system.-  Consider  the  axis  system  (Xf,Yf,Zf  in  fig.  5)  which  is 
fixed  relative  to  the  forearm.  For  the  degenerate  condition  of  the  robot  arm  in 
figure  5,  Xf,  Yf,  and  Zf  are  aligned  with  X5,  Z4,  and  Z3,  respectively.  This 
axis  system  and  the  limits  in  table  I  for  the  wrist  joint  angles  84,  85,  and  0g 
are  shown  in  figure  6. 

Coordinated  movement.-  Given  that  the  robot  wrist  is  in  (or  nearly  in)  the 

degenerate  position  shown  in  figure  5,  suppose*  that  an  operator  wants  to  rotate  the 

robot  hand  about  the  direction  indicated  by  Q  ,  which  makes  an  angle 

c 


P  =  tan 


(30) 
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with  the  X6  axis.  04  is  varied  in  figure  7  to  make  X5  normal  to  Q ^  (which 

places  Z4  along  ^  ).  Meanwhile,  06  is  varied  at  the  same  rotational  speed  in 

the  opposite  direction  to  04  to  cancel  any  rotation  of  the  robot  hand  that  would 

result  from  varying  04.  The  objective  is  to  choose  the  smallest  variation  in  04 

which  will  locate  Xc  normal  to  Q  and  yet  not  violate  the  limits  on  either  04 

J  c 

or  0g.  There  are  four  possibilities,  depending  on  the  value  of  04  when  the  move¬ 
ment  is  initiated.  In  figure  8,  the  possible  variations  in  04  are  denoted  by 


61  = 

0  +  P  +  90° 

6 

(31) 

62  = 

_jy> 

1 

— * 

00 

0 

0 

(32) 

63  = 

61  -  360° 

(33) 

64  = 

360°  +  &2 

(34) 

The  coordinated  movement  is  explained  as  follows:  whenever  85  is  close  to  0° 
(by  a  specified  amount),  equations  for  the  coordinated  movement  are  used.  Let 

(|>,  =  0  +  6,  (35) 

1  4  1 

=  0 l  -  6.  (36) 

x  6  1 

for  i  =  1 ,  2,  3,  4  be  paired  candidate  values  of  04  and  0g  for  the  coordinated 
movement.  Let  6^  be  the  least  6i  (eqs.  (31)  to  (34))  in  absolute  value  such  that 

j 4^ |  <  135°  and  |  |  <  270°;  that  is,  a  coordinated  movement  is  possible.  Then, 

84  .  ^  07, 

e6  .  V  <38, 

are  the  new  target  values  of  04  and  06  for  the  coordinated  movement.  In  this 
movement,  which  is  automated,  04  and  06  are  varied  in  unison  at  a  selected  joint 
angle  rate  to  the  target  values.  Once  this  coordinated  movement  to  the  intermediate 
orientation  is  completed,  the  robot  hand  rotation  is  accomplished  by  setting 

0  =  Q  (-1)k+1  (39) 

3  C 


1 1 


where  Qc  is  the  magnitude  of  2,.  For  example,  if  k  =  1,  then  6-,  is  the  optimal 

variation  in  64  in  figure  8*  This  means  that  will  be  placed  90°  counterclock¬ 
wise  from  0^  and  Z4  will  be  aligned  with  Hence,  a  positive  rotation  about 

z4  to  make  0  equal  to  the  magnitude  of  8  will  produce  the  commanded  rotational 
vector  y  •  c 

c 

With  no  joint  limits  (continuous  rotational  joints)  on  04  and  06,  6.  always 
exists  so  that  a  coordinated  movement  is  possible,  but  with  limits  on  04  and  06, 
there  are  orientations  from  which  the  movement  cannot  be  made.  In  these  cases,  the 
operator  may  choose  to  control  the  wrist  joints  separately. 

An  operator  should  be  given  a  backup  option  to  individually  control  the  three 
wrist  joint  rates  0^,  0,.,  and  06»  This  option  bypasses  mathematical  singulari¬ 

ties  in  wrist  rotation.  A  disadvantage  of  this  mode  is  that  the  operator  must  now 
think  about  how  his  changes  in  the  wrist  joint  angles  will  affect  the  robot  hand 
movement. 


EXAMPLE 

Figure  9  shows  three  sequential  orientations  of  the  robot  hand  axis  system 
(x6'y6'z6*  and  commanded  rotational  rates  of  the  robot  hand.  In  figure  9(a),  the 
operator  commands  a  rotation  (Wy)c  about  the  Yg-axis  to  raise  the  Zg-axis  to  the 
vertical  position  shown  in  figure  9(b).  In  figure  9(b),  the  operator  then  commands  a 
rotation  about  Xg  to  tilt  Zg  as  shown  in  figure  9(c).  The  robot  wrist  is  in  its 
degenerate  position  in  figure  9(b).  A  small  commanded  rotational  rate  of  1  deg/sec 
is  chosen  for  illustration. 


Using  Resolved-Rate  Control  With  Maximum  Rate 


Table  II  shows  the  commanded  hand  rotational  rates  and  the  resulting  wrist  joint 
angles,  wrist  joint  angle  rates,  and  computed  robot  hand  rotational  rates  as  a  func¬ 
tion  of  time  for  resolved-rate  control  with  rate  limiting.  At  t  =  1.5  sec,  the 
robot  wrist  is  positioned  as  shown  in  figure  9(b),  which  is  the  degenerate  position. 
Note  that  if  05  were  ever  exactly  0,  equation  (15)  would  be  indeterminant.  The 

magnitudes  of  0^  and  0^  are  held  at  their  maximum  allowable  value  (which,  in  this 

paper,  is  L  =  3. 5  rad/sec)  until  0^  is  such  that  computed  speeds  no  longer  have 
magnitudes  which  exceed  this  maximum  speed  value.  While  moving  out  of  the  degenerate 
region  (05  =  0°),  increases  toward  1?  but  at  the  same  time,  Uy  takes  on  values 

as  large  as  32  percent  of  the  commanded  value  (Wx)c  =  1»  This  may  result  in  unde¬ 
sirable  motion  of  the  robot  hand,  especially  in  the  performance  of  a  delicate  task. 
After  05  «  -0.23°,  the  performance  approaches  the  desired  motion.  Time  is  the  ref¬ 
erence  parameter  in  table  II  but  is  probably  not  as  important  as  acconplishing  the 
desired  movements  of  the  robot  hand.  For  example,  it  may  not  matter  whether  the 
robot  hand  reaches  the  position  in  figure  9(c)  in  3  sec  or  4  sec. 


12 


Using  Generalized  Matrix  Inverse 


In  table  III,  the  robot  hand  moves  as  commanded  up  to  t  =  1.5  sec*  Thereafter, 
the  robot  hand  rotates  with  =  -1  deg/sec  rather  than  with  the  commanded  value 
(co^)c  =  1  deg/sec. 


Using  Resolved-Rate  Control  With  Coordinated  Wrist 

Figure  10  shows  how  the  robot  hand  is  maneuvered  with  the  intermediate  wrist 
movement,  and  table  IV  gives  rotational  information  as  a  function  of  time.  From 
t  =  0  to  t  =  1.5  sec,  the  robot  hand  rotates  to  the  vertical  position  ( 65  =  0°). 
From  t  =  1.5  sec  to  t  =  2.05  sec,  opposite  rotations  of  64  and  0g  at  maximum 
joint  angle  rates  (or  any  assigned  rates)  locate  the  robot  wrist  for  tilting  to  the 
desired  position  (t  =  3.55  sec).  There  is  a  brief  delay  for  the  wrist  to  position 
itself  (without  moving  the  robot  hand)  in  order  to  move  the  robot  hand  as  commanded. 
For  65  =  0°,  there  are  no  extraneous  hand  movements  during  the  coordinated  wrist 
movement,  which,  consequently,  appears  to  enhance  the  dexterity  of  the  robot  arm  and 
to  provide  a  beneficial  method  of  control. 


CONCLUDING  REMARKS 

The  robot  wrist  which  orients  the  robot  hand  consists  of  three  rotational 
joints.  When  two  of  these  joints  are  colinear  so  that  they  produce  identical  rota¬ 
tions  of  the  hand,  a  degree  of  freedom  is  lost,  and  a  singular  or  degenerate  con¬ 
dition  exists.  The  usual  approach  of  avoiding  this  degenerate  wrist  condition 
restricts  the  wrist  workspace  and  requires  additional  task  planning.  This  paper 
deals  with  methods  to  control  the  robot  hand  at  or  near  this  degenerate  condition. 

The  following  comments  apply  in  the  degenerate  region  of  the  wrist  joints: 

1.  Resolved-rate  control  results  in  excessive  wrist  joint  angle  rates.  Setting 
the  excessive  rates  equal  to  some  maximum  operational  limit  also  leads  to  erratic 
behavior.  For  example,  in  some  cases,  the  robot  hand  will  not  respond  at  all  to 
commands.  In  other  cases,  a  rate  occurs  about  one  hand  axis  when  the  command  is  for 
a  rate  about  another  axis. 

2.  The  generalized  matrix  inverse  can  produce  unwanted  responses  of  the  robot 

hand. 

3.  Resolved-rate  control  with  a  coordinated  movement  of  the  wrist  near  the  wrist 
degenerate  condition  appears  to  enhance  the  dexterity  of  the  robot  arm.  The  method 
uses  a  coordinated  movement  of  the  first  and  third  joints  of  the  robot  wrist  to 
locate  the  second  wrist  joint  axis  for  movement  of  the  robot  hand  in  the  commanded 
direction.  This  method  does  not  entail  infinite  joint  angle  rates  or  cause  extrane¬ 
ous  hand  movements;  however,  there  is  a  brief  delay  for  the  coordinated  movement 
(which  is  accomplished  in  an  optimal  manner).  In  a  delicate  situation  (for  example, 
peg-in -the -hole  task),  this  method  of  control  may  be  beneficial.  In  addition  to 
teleoperator  systems,  the  coordinated  movement  can  be  implemented  in  higher  level 
control  systems.  Also,  with  joint  limits,  there  are  orientations  from  which  a 
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coordinated  movement  cannot  be  made.  In  these  cases,  the  operator  may  choose  to 
control  the  individual  joint  angles  separately. 


Langley  Research  Center 

National  Aeronautics  and  Space  Administration 
Hampton,  VA  23665 
June  1 ,  1  984 
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APPENDIX  A 


HOMOGENEOUS  TRANSFORMATION  MATRICES 

When  the  parameters  in  table  I  are  introduced  into  the  general  transformation 
matrix  in  equation  (1),  the  following  six  transformation  matrices  (which  are  the  same 
as  those  used  in  ref.  4)  result: 


A^  = 


A,  = 


-cos  0^ 
-sin  0^ 
0 
0 


-sin  e2 

COS  0^ 

2 

0 

0 


0 

0 

1 

0 

-cos  8„ 
-sin  0„ 
0 
0 


-sin  0 


1 


1 


cos  0^ 
0 
0 


0 

0 

1 

0 


0 
0 

NO 
1  _ 

-ES  sin  0, 

4. 

ES  cos  0^ 
2 

SN 

1 


(A1  ) 


(A2) 


A„  = 


A^  = 


A.  = 


A_  = 


-sin  e3 

COS  0^ 

0 

0 


-cos  0„ 
4 

-sin  0,, 
4 

0 

0 


-cos  0^ 
-sin  e5 
0 
0 


COS  0,. 
6 

sin  0^ 
€ 

0 

0 


0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

1 

0 


-sin  0,. 
6 

cos 


0 

0 


cos  0^ 
sin  e3 
0 
0 

-sin  0„ 
A 

cos  0„ 

4 

0 

0 

-sin  0C 
cos  0^ 
0 
0 

0 

0 

1 

0 


0 

0 

0 

0 

0 

WE 
1  _ 

<r 

0 

0 

1_ 

0 

0 

HW 

1 


(A3) 


( A4 ) 


(A5) 


( A6) 
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ROTATIONAL  VELOCITY  OF  ROBOT  HAND  IN  ROBOT  HAND  AXIS  SYSTEM 

Additional  symbols  used  in  this  appendix  are  defined  as  follows: 

E.j,E2,E3  projection  of  rotational  velocity  attributed  to  joints  1 ,  2,  and  3 

along  X3,  Y3,  and  Z3,  respectively 

expression  for  certain  elements  in  r|  defined  by  equations  (B8), 
(B9),  (B10)f  and  (B11),  respectively 

The  rotational  velocity  of  the  robot  hand 


0) 


,2,3 


+  0) 


4,5,6 


(B1  ) 


is  the  summation  of  the  individual  contributions  from  the  different  joints  in  the 
robot  arm*  Expressing  these  contributions  in  the  robot  hand  axis  system  is  straight¬ 
forward,  but  for  convenience,  some  expressions  for  these  contributions  are  contained 
in  this  appendix. 


Rotational  Velocity  From  Joints  1,  2,  and  3 


Rather  than  use  equation  (9)  in  the  text,  express  the  rotational  velocity  from 
joints  1,  2,  and  3  in  a  more  computationally  efficient  way  as 


r 

0 

^0 

ro 

R2 

0 

>+( 

0 

:* 

0 

> 

l 

_ 

\ 

Lei  J 

l02  J 

, 

le3J 

(B2 ) 


Equation  (B2)  is  partially  expanded  to 


C  > 
Ei 


1,2,3 


"WE2 


(B3 ) 


E3 

V.  V 


where 


E1  =  -01  Sin(02  +  V 


(B4) 
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E2  =  02  +  93 


E3  =  91  COS  ^2  + 


Then,  with 


cos  0.  sin  0- 
4  5 


sin  0^  sin  0^ 


-sin  0^  cos  0£ 
sin  ©5  sin 
cos  0^ 


where 


O  =  cos  9  cos  6^  cos  0^  -  sin  9  sin  9 

*•  -i  A  R  K  tx  c 


o  =  sin  9  cos  0  cos  9c  +  cos  0  sin  0 
*2  4  5  6  4  6 


Q  =  -cos  04  cos  ©5  sin  ©6  -  sin  ©4  cos  0g 


(B1  0) 


Q4  =  -sin  ©4  cos  0^  sin  0^  +  cos  ©4  cos  0^ 


(B1 1) 


equation  (B3)  becomes 


1,2,3 


Q1E1  +  Q2E2  ‘  Sln  9 5  C°S  9 6  E3 

Q3E1  +  Q4E2  +  Sin  95  Sin  96  E3 
cos  04  sin  05  E1  +  sin  04  sin  ©5  E2  +  cos  ©5  E3 


(B12) 


Rotational  Velocity  From  Joints  4,  5,  and  6  (Wrist  Joints) 
Equation  (10)  in  the  text  is  better  expressed  as 


4,5,6 


r6r5r4<°  yr 


+  /  0 


(B13) 
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which  expands  to 


r . 

A 

-e4 

sin 

0^  cos 

e6+  K 

sin 

06 

- 

• 

04 

sin 

05  sin 

96+ 

cos 

06 

• 

0.  COS 

4 

05  + 

J 

Assumption 

To  simplify  the  analysis  in  this  paper,  assume  that  0=0=0= 
2)1,2, 3  =  0  in  equation  (B12).  Hence,  equation  (B1 )  reduces  to  2  3 


. 

• 

\ 

-04 

sin  0C  cos  0.  +  0_  sin 
5  6  5 

06 

r 

• 

04 

• 

sin  0_  sin  0_  +  0_  cos 
5  6  5 

06 

& 

0.  cos  0C  +  0^ 

4  5  6 

J 

Equation  (B15)  is  solved  for  the  joint  speeds  as  indicated  by  equations  ( 
and  (16)  in  the  text. 


(B14) 


so  that 


(B15) 


4),  (15), 
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TABLE  I.-  HOMOGENEOUS  TRANSFORMATION  MATRIX  PARAMETERS 


[From  ref.  4] 


Joint, 

i 

«i  f 
deg 

ai 

ri 

6i' 

deg 

0^  limits, 
deg 

1 

2 

90 

0 

0 

ES  + 

NO* 

SN* 

0!  +  180 
©2  +  90 

±160 

±165 

3 

90 

0 

o, 

WE* 

03  +  90 

±135 

4 

90 

0 

e4  +  180 

±135 

5 

90 

0 

°ll 

HW 

9c  +  180 

±105 

6 

0 

0 

06 

±270 

*NO  =  Neck-to-base  distance  -  66.05  cm  =  26  in. 

*ES  =  Elbow- to-shoulder  distance  =  43.18  cm  =  17  in. 
SN  =  Shoulder-to-neck  distance  =  15.24  cm  =  6  in. 
*WE  =  Wrist-to-elbow  distance  =  43.15  cm  =  17  in. 

HW  =  Hand-to-wrist  distance  -  15.24  cm  =  6  in.  (In 
the  kinematic  equations,  the  distance  from  the  hand  axis 
system  to  the  wrist  is  assumed  to  be  0.) 


TABLE  II.-  RESOLVED-RATE  CONTROL  WITH  RATE  LIMITING 


TABLE  IV.-  RESOLVED-RATE  CONTROL  WITH  COORDINATED  WRIST  MOVEMENT 


Figure  3.-  Initial  position  of  robot  arm,  joint  axis  systems,  and 
commanded  robot  hand  velocities. 


(a)  (u>y/M)c  =  0* 

Figure  4*-  Rotational  velocity  of  robot  hand  versus  commanded 
velocities  for  resolved-rate  control  with  rate  limiting. 

M  is  maximum  angular  wrist  joint  velocity;  0c  =  0°. 


Excluded  region  of  0^ 

0,  =  -135 
4 


Figure  6 


Figure  8.-  Angles  used  in  choosing  oj 
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(c)  Final  orientation. 

Figure  9.-  Three  sequential  wrist  orientations  and  commanded  rotational  speeds  in 

robot  hand  axis  system. 


(b)  Second  orientation  and  coordinated 

(a)  Initial  orientation.  joint  angle  rates  (equal  magnitude 

but  opposite  directions). 


(c)  Intermediate  orientation.  (d)  Final  orientation. 

Figure  10.-  Maneuvering  robot  hand  axis  system  by  using  coordinated  wrist  rotation. 
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the  degenerate  robot  wrist.  The  method  uses  a  coordinated  movement  of  the  first  and 
third  joints  of  the  robot  wrist  to  locate  the  second  wrist  joint  axis  for  movement  of 
the  robot  hand  in  the  commanded  direction.  There  is  a  brief  delay  for  the  coordi¬ 
nated  movement  (which  is  accomplished  in  an  optimal  manner).  The  method  does  not 
entail  infinite  joint  angle  rates  and  appears  to  be  a  beneficial  method  of  control. 
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